#include "commu.h"
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"

static const char *TAG = "commu";
static QueueHandle_t commu_queue = NULL;
#define COMMU_QUEUE_SIZE 32
#define COMMU_STACK_SIZE (4 * 1024)

#include "config.h"
#ifndef USE_SYS_MEM
#include "m_memory.h"
extern uint8_t memory_pool[16 * 1024];
extern MemoryPoolManager pool;
#endif

extern int commu_route(uint8_t *buf, uint8_t len);

int SendToCommu(uint8_t *buf, uint8_t len)
{
#ifdef USE_SYS_MEM
    commu_msg_t *msg = (commu_msg_t*)malloc(sizeof(commu_msg_t));
#else
    commu_msg_t *msg = (commu_msg_t*)mempool_alloc(&pool, sizeof(commu_msg_t));
#endif
    if (msg == NULL) {
        return 1;
    }
    msg->len = len;
#ifdef USE_SYS_MEM
    msg->buf = (uint8_t *)malloc(len);
#else
    msg->buf = (uint8_t*)mempool_alloc(&pool, len);
#endif
    if (msg->buf == NULL) {
#ifdef USE_SYS_MEM
        free(msg);
#else
        mempool_free(&pool, msg);
#endif
        return 1;
    }
    memcpy(msg->buf, buf, len);
    if (xQueueSend(commu_queue, &msg, portMAX_DELAY) != pdTRUE) {
        ESP_LOGE(TAG, "queue send fail");
        return 2;
    }
    return 0;
}

static void commu_task(void *pvParameters)
{
    for (;;) {
        commu_msg_t *msg;
        if (xQueueReceive(commu_queue, &msg, (TickType_t)portMAX_DELAY) != pdTRUE) {
            ESP_LOGE(TAG, "queue receive fail");
        }
        ESP_LOGI(TAG, "len:%d data:%02X%02X%02X%02X", msg->len, msg->buf[0], msg->buf[2], msg->buf[3], msg->buf[4]);
        if (commu_route(msg->buf, msg->len) != 0) {
            ESP_LOGE(TAG, "route fail");
        }
#ifdef USE_SYS_MEM
        free(msg->buf);
        free(msg);
#else
        mempool_free(&pool, msg->buf);
        mempool_free(&pool, msg);
#endif
        vTaskDelay(pdMS_TO_TICKS(10));
    }
    vTaskDelete(NULL);
}

void commu_init(void)
{
    ESP_LOGI(TAG, "init");
    commu_queue = xQueueCreate(COMMU_QUEUE_SIZE, sizeof(commu_msg_t));
    if (commu_queue == NULL) {
        ESP_LOGE(TAG, "queue init fail");
    }
    xTaskCreate(commu_task, "commu", COMMU_STACK_SIZE, NULL, 14, NULL);
}